import pygame
import animate_curve

anima = animate_curve.Animation_MP("PID", ["Error", "derivative", "integral", "output"])

pygame.init()
screen_width = 800
screen_height = 600
ball_radius = 5
# screen = pygame.display.set_mode(
#    (screen_width+anima.canvas.get_width_height()[0], max(screen_height, anima.canvas.get_width_height()[1])))
screen = pygame.display.set_mode((screen_width, screen_height))
# pygame.display.set_caption("Ball with Damping")
clock = pygame.time.Clock()

# 定义 PID 控制器参数
Kp = 0.1  # 比例增益
Ki = 0  # 积分增益
Kd = 0  # 微分增益
d_filter = 500

# 定义系统响应参数
Ts = 1/100 # 采样时间 (s)

def low_pass_filter(input_data):
   if abs(input_data) < d_filter:
      return input_data
   else:
      return d_filter * input_data / abs(input_data)

def draw_ball(screen, x, y, ball_radius):
   pygame.draw.circle(screen, (255, 255, 255), (int(x), int(y)), ball_radius)

# 定义 PID 控制器
def pid_controller(error, integral, derivative):
   return Kp * error + Ki * integral + Kd * derivative

# 定义模拟系统
def simulated_system(setpoint, current_output):
   return setpoint - current_output


# 定义 PID 控制循环
def pid_control_loop(setpoint, initial_output):
   current_output = initial_output
   integral = 0
   derivative = 0
   previous_error = None
   running = True
   y = 100
   print("start")
   while running:
      error = simulated_system(setpoint, current_output)
      if previous_error is not None:
         integral += error * Ts
         derivative = (error - previous_error) / Ts
         derivative = low_pass_filter(derivative)

      output = pid_controller(error, integral, derivative)
      current_output += output
      previous_error = error
      anima.update([error, derivative, integral, output])
      # print(f"{current_output:.2f} {error:.2f}")
      # print(f"{error:.2f},{previous_error},{integral:.2f},{derivative:.2f}")
      # 更新屏幕上的小球
      screen.fill((0, 0, 0))
      draw_ball(screen, current_output // 10, y, ball_radius)
      # screen.blit(anima.surface, (screen_width, 0))
      pygame.display.flip()
      clock.tick(1/Ts)
      for event in pygame.event.get():
         if event.type == pygame.QUIT:
            running = False
            pygame.quit()
            anima.quit()
         if event.type == pygame.MOUSEMOTION:
            pygame.display.set_caption(f"{event.pos[0] * 10}, {event.pos[1]}")
         if event.type == pygame.MOUSEBUTTONDOWN:
            setpoint = event.pos[0] * 10
            y = event.pos[1]
      

if __name__ == "__main__":
   setpoint = 100
   initial_output = 0.0
   anima.start()
   pid_control_loop(setpoint, initial_output)